#include <yaml_test/yaml_function.h>
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <sys/time.h>
#include <stdio.h>  
#include <time.h> 
std::string yaml_add = "/home/bingo/study_ws/src/yaml_test/yaml_test.yaml";       //yaml文件所在的路径..
//catkin_make -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes
//可以自定义模板的，但是感觉有点复杂，暂时先用着
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>  MoveBaseClient;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "send_goal_node");
    ROS_INFO("Start!");
    MoveBaseClient ac("move_base", true);
    // Wait 60 seconds for the action server to become available
    ROS_INFO("Waiting for the move_base action server");
    ac.waitForServer(ros::Duration(60));
    ROS_INFO("Connected to move base server");

    ROS_INFO("Read Yaml");
    geometry_msgs::Pose GET_POSE;
    std::string address_name = "hospital";
    YAML::const_iterator yaml_it = YAML_FUNCTION::Get_Specified_Address_Pose(yaml_add,address_name,GET_POSE);
    std::cout << "GET_POSE.position.x:"  << GET_POSE.position.x<< std::endl;
    std::cout << "GET_POSE.position.y:"  << GET_POSE.position.y<< std::endl;
    std::cout << "GET_POSE.position.z:"  << GET_POSE.position.z<< std::endl;
    std::cout << "GET_POSE.orientation.w:"  << GET_POSE.orientation.w<< std::endl;
    std::cout << "GET_POSE.orientation.x:"  << GET_POSE.orientation.x<< std::endl;
    std::cout << "GET_POSE.orientation.y:"  << GET_POSE.orientation.y<< std::endl;
    std::cout << "GET_POSE.orientation.z:"  << GET_POSE.orientation.z<< std::endl;

    // Send a goal to move_base
    //目标的属性设置
    ROS_INFO("Read Yaml");
    move_base_msgs::MoveBaseGoal goal; 
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x = GET_POSE.position.x;
    goal.target_pose.pose.position.y = GET_POSE.position.y;
    goal.target_pose.pose.position.z = GET_POSE.position.z;
    goal.target_pose.pose.orientation.w = GET_POSE.orientation.w;
    goal.target_pose.pose.orientation.x = GET_POSE.orientation.x;
    goal.target_pose.pose.orientation.y = GET_POSE.orientation.y;
    goal.target_pose.pose.orientation.z = GET_POSE.orientation.z;

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);
    // Wait for the action to return
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("You have reached the goal!");
    }
    else
    {
        ROS_INFO("The base failed for some reason");     
    }
    




    return 0;
}
